import numpy as np

'''
时间对齐
初始时间对齐: 默认aero和tension同时采集, tension的时间戳作为起始时间
'''
def time_cut(aero, f_aero, tension, f_tension, encoder, f_encoder, angle, f_angle):

    start_time = tension[0,-1]
    angle_time = angle[:,-1]

    delta_time = np.abs(angle_time-start_time)
    time_id = np.where(delta_time==min(delta_time))[0][0]
    
    step = int(f_angle/f_aero)

    angle = angle[time_id:time_id+step*len(aero):step,:]

    
    # print(aero.shape)
    # print(tension.shape)
    # print(encoder.shape)
    # print(angle.shape)

    print("绳拉力起始时间："+str(tension[0,-1]))
    print("陀螺仪起始时间："+str(angle[0,-1]))
    print("绳拉力终止时间："+str(tension[-1,-1]))
    print("陀螺仪终止时间："+str(angle[-1,-1]))

    print("------")

    
    return aero, tension, encoder, angle


'''
时间对齐2
取最短的值作为长度
'''
def time_cut2(aero_w,aero_nw,angle_w,angle_nw):
    length = min(len(aero_w),len(aero_nw),len(angle_w),len(angle_nw))

    aero_w = aero_w[0:length,:]
    aero_nw = aero_nw[0:length,:]
    angle_w = angle_w[0:length,:]
    angle_nw = angle_nw[0:length,:]

    return aero_w,aero_nw,angle_w,angle_nw


def time_cut1(aero, f_aero, tension, f_tension, encoder, f_encoder, angle, f_angle):

    step = int(f_angle/f_aero)
    angle = angle[0:0+step*len(aero):step,:]

    print("------")

    
    return aero, tension, encoder, angle